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摘 要 : 针对 机 器 人 导航 无 迹 快速 同步 定位 与 地 图 构建 Unscented FastSLAM) 算法 由 于 重 采 样 造成 样本 粒子 退化 ， 

进而 导致 估计 精度 下 降 的 问题 ,提出 一 种 基于 自 适 应 渐 消 无 迹 粒子 滤波 的 Unscented FastSLAM 算法 。 该 算法 将 无 迹 粒 
子 滤波 与 渐 消 滤波 相 融 合 产生 自 适应 建议 分 布 函数 ， 同 时 将 粒子 根据 权 值 进行 优化 组 合 ， 仅 对 组 合 后 的 部 分 不 稳定 的 

粒子 进行 系统 重 采 样 。 通 过 这 两 方面 使 系统 具有 高 度 自 适应 性 的 同时 保证 粒子 的 多 样 性 ， 缓 解 粒 子 的 退化 现象 。 仿 真 

实验 表明 ， 提 出 算法 与 Unscented FastSLAM 算法 相 比 ， 可 以 用 较 少 的 粒子 实现 更 高 的 SLAM 的 估计 精度 ， 很 大 程度 

上 降低 了 SLAM 算法 的 复杂 度 。 

关键 词 : 同步 定位 与 地 图 构建 ; 粒子 退化 ; 自 适应 渐 消 无 迹 粒子 滤波 ; 自 适应 部 分 系统 重 采 样 

中 图 分 类 号 : TP242.6 doi: 10.3969/j.issn.1001-3695.2017.11.0742 


Unscented fastSLAM algorithm based on adaptive fading unscented particle filter 


Wang Qian'’, Zeng Qingjun'’, Zhang Jiaminla, Yao Jinyil’, Zhou Qirun!'®, Dai Xiaoqiangla? 
(1. a. School of Electronic & Information, b. School of Computer Science & Engineering Jiangsu University of Science & 
Technology, Zhenjiang Jiangsu 212003, China; 2. Jiangsu Shiptek Automation Technology Co, Ltd, Zhenjiang Jiangsu 212003， 
China) 


Abstract: For the unscented fast simultaneous localization and mapping (Unscented FastSLAM) algorithm in robot navigation, 
the sample particle is degraded due to resampling, which leads to the problem of reduced accuracy. In order to solve the problem, 
this paper developed an improved Unscented FastSLAM algorithm based on adaptive fading unscented particle filter. The 
algorithm merged the unscented particle filter with the fading filter to form adaptive proposed distribution function. At the same 
time, the particles were optimally combined according to their weight, and only unstable particles are resampled. Through these 
two aspects could make the system highly adaptive, while ensuring the diversity of particles and mitigating the degradation of 
particles. Simulation experiments show that compared with Unscented FastSLAM algorithm, the proposed algorithm can achieve 
higher SLAM estimation accuracy with fewer particles, which greatly reduces the complexity of SLAM algorithm. 
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出 的 基于 粒子 滤波 器 (particle filtering, PF) 的 FastSLAM 方法 
在 处 理 非 线性 、 非 高 斯 系统 上 具有 很 大 优势 品 。 在 标准 
同步 定位 与 地 图 构建 (simultaneous localization and mapping， FastSLAM 中 ， 粒 子 滤波 器 用 于 机 器 人 位 置 估 计 ， 扩 展 卡尔 曼 
SLAM) 是 实现 机 器 人 完全 自主 导航 的 关键 问题 。 具体 描述 为 : 滤波 器 (extendedKalman filtering, EKF ) 用 于 特征 位 置 估计 外。 
机 器 人 在 部 分 或 完全 未 知 的 环境 中 ， 仅 利用 自身 携带 的 传感器 因此 ，FastSLAM 存在 由 雅 可 比 扼 阵 的 推导 和 非 线 性 函数 的 线 
感知 环境 信息 ， 构 建 环 境地 图 的 同时 确定 自身 的 位 置 02。 生 近 似 引 起 的 估计 不 准确 的 问题 。 为 了 解决 这 些 问题 ， 文 献 [5] 
解决 SLAM 问题 的 方法 有 很 多 , 其中, Montemerlo 等 人 提 ” ”提出 了 Unscented FastSLAM (UFastSLAM) 方法 。 其 中 ,无 迹 
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粒子 滤波 (unscented particle filtering，UPF ) 用 于 机 器 人 路 径 估 
计 ， 无 迹 卡 尔 曼 滤波 (unscented Kalman filtering，UKF ) 用 于 
特征 位 置 估计 。 通 过 应 用 无 迹 变 换 来 消除 雅 可 比 计算 的 线性 化 
过 程 ， 因 此 状态 估计 的 准确 性 有 显著 改善 四。 

但 是 UFastSLAM 在 重 采 样 时 只 复制 大 权 值 粒子 ， 抛 弃 小 
权 值 粒子 ， 导 致 粒子 集 出 现 贫 化 现象 ， 使 得 被 抛弃 粒子 中 所 携 
带 的 机 器 人 位 置 及 地 图 特征 历史 信息 同时 丢失 叫 。 针 对 这 一 过 
程 导致 估计 精度 下 降 的 问题 , 文献 [8] 提 出 基于 平方 根 无 迹 卡 尔 
曼 滤 波 (square root unscented Kalman filtering，SR-UKF) 的 
FastSLAM 算法 ， 使 用 SR-UKF 估计 每 个 粒子 的 后 验 位 姿 建议 
分 布 , 提高 粒子 采样 精度 。 文 献 [9] 提 出 基于 返 代 EKF 建议 分 布 
和 线性 优化 重 采样 的 FastSLAM 算法 ， 使 粒子 分 布 在 高 观测 似 
然 区 域 ， 同 时 缓解 粒子 退化 现象 。 
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UFastSLAM 算法 的 最 后 一 步 就 是 根据 粒子 的 权 值 大 小 对 
粒子 进行 重 采 样 。 


2 基于 自 适应 渐 消 UPF 的 UFastSLAM 方法 


UFastSLAM 算法 依然 存在 粒子 退化 现象 , 除了 重 采 样 使 得 
粒子 退化 ， 从 根源 上 说 ， 粒 子 退 化 是 由 于 所 使 用 的 粒子 
从 后 验 分 布 中 采样 而 来 ， 而 是 从 一 个 近似 后 验 分 布 的 建议 分 布 
函数 中 采样 得 到 的 。 因 此 ， 随 着 算法 不 断 兴 代 、 重 采样 的 不 断 


因此 ,本 文 将 从 建议 分 布 和 重 采 样 两 方面 对 UFastSLAM 进 
行 改进 : 一 方面 , 引入 文献 [10 一 12] 中 的 渐 消 滤波 与 无 迹 粒子 滤 
波 相 结合 产生 参数 自 适 应 调节 的 建议 分 布 函数 ， 从 而 使 得 粒子 
的 分 布 更 加 符合 系统 的 后 验 概率 分 布 ， 降 低 粒 子 的 退化 程度 ; 
另 一 方面 ， 在 重 采样 过 程 中 将 粒子 权 值 按 大 小 进行 分 类 并 优化 
组 合 ， 仅 对 部 分 粒子 进行 系统 重 采 样 ， 从 而 自 适 应 调整 重 采 样 
粒子 数 ， 增 加 粒子 的 多 样 性 。 通 过 以 上 方法 提高 系统 的 自 适 应 
性 ， 使 机 器 人 位 置 和 特征 位 置 估计 更 加 精确 。 


1 ”机 器 人 UFastSLAM 算法 


UFastSLAM 算法 的 基本 思想 是 以 基本 的 FastSLAM 算法 
为 框架 ， 将 系统 的 状态 估计 分 解 为 对 机 器 人 运动 路 径 的 估计 和 
对 环境 特征 位 置 的 估计 两 个 部 分 03， 同 时 使 用 UKF 取代 一 阶 
线性 化 和 EKF 的 过 程 ， 表 示 为 


ps ,91z ,n,n )= p(s lzu',n') I Ip(0, |s',z ,un ) (1) 
id 


路 标 位 置 估 计 CUKF) 


路 径 后 验 估计 (UPF) 


其 中 :$={5,…5) 为 机 器 人 的 路 径 估 计 ，s, 为 1 时 刻 的 机 器 人 
位 姿 ; 98={9,…0,} 为 路 标 位 置 的 合集 ; z={a…4) 和 
w= 人 tas…t4} 分 别 为 1 时 刻 的 观测 量 和 控制 输入 ; r=,…,n} 为 
数据 关联 表 ， 每 个 及 代表 1 时刻 观测 到 的 路 标的 索引 。 


UFastSLAM 算法 采用 一 个 无 迹 粒 子 滤波 器 对 机 器 人 路 径 


进行 估计 ， 其 中 每 个 粒子 都 包含 一 份 地 图 信息 ， 再 用 N 个 独立 
的 路 标 估计 来 进行 地 图 估计 ， 每 个 路 标 都 采用 一 个 UKF 来 估 


计 。 因 此 , 假设 在 UFastSLAM 算法 中 的 粒子 数 是 M 个 ,那么 
UKEF 的 数量 就 是 MxN 个 , 则 :时 刻 第 ;个 粒子 的 组 成 表达 式 为 


中 {9 二 更,… 击 , 弄 | © 


其 中 : si 为 第 i 个 粒子 对 机 器 人 的 路 径 估计 ; yi 和 于 分 别 代 


表 在 第 i 个 粒子 包含 的 地 图 中 第 NN 个 路 标的 均值 和 协 方差 。 
在 UFastSLAM 算法 中 ， 机 器 人 位 姿 通 过 式 (3) 采 样 得 到 


进行 ， 粒 子 退 化 无 法 避免 。 针 对 以 上 问题 ， 提 出 基于 自 适应 渐 
消 UPF 建议 分 布 的 UFastSLAM 算法 ， 同 时 采用 自 适应 部 分 系 
统 重 采 样 方 法 对 粒子 集 进 行 重 采样 。 
2.1 自 适应 渐 消 UPF 建议 分 布 
适应 渐 消 UPF (后 文 简称 AF-UPF ) 以 粒子 滤波 为 基本 
框架 ， 同 时 将 渐 消 滤波 与 UT 变换 相 结合 产生 参数 可 自 适 应 调 
节 的 建议 分 布 函数 , 充分 利用 系统 最 新 观测 信息 中 的 有 效 信息 ， 
使 得 建议 分 布 更 加 接近 真实 后 验 分 布 ， 对 过 程 中 参数 变动 有 更 
好 的 自 适 应 性 。 
考虑 非 线 性 动态 系统 : 
w= (ww) 

w=h(%,v) (5) 
其 中 : x 、z 分 别 为 1 时 刻 系统 的 状态 向 量 和 观测 向 量 ，w、 
v, 为 具有 零 均 值 和 协 方差 分 别 为 8, 和 BR 的 过 程 噪声 和 观测 噪 
声 。 则 AF-UPF 算法 描述 如 下 : 
a) 状态 增 广 。 


T 


; ; -TT ; 。 
ai Ty [Ty i 1 
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Pi 0 0 
Zi = QQ 0 
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(6) 


其 中 : x 为 增 广 状态 向 量 ; 8 为 增 广 协 
方差 矩阵 。 

b) 通过 对 称 采 样 得 到 工 =2N+1 个 sigma 点 ，W 为 估计 向 
量 的 维 数 


为 先 验 状 态 向 量 ， 


Pa = 
ts) (et 
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其 中 : 为 矩阵 的 第 列 ，4=a? (N+x)-N 为 尺度 参数 ， 常 量 
a(0<a<]) 决定 了 sigma 点 在 均值 附近 的 分 布 ，* 是 次 级 尺度 
调节 参数 ， 一 般 设置 为 0。 这 些 sigma 点 通过 非 线性 函数 进行 


201804.02061v1 


chinaXiv 


录用 稿 


传递 ， 产 生 预 测 的 sigma 样本 点 : 


ZZ 党 (2 (8) 


上 上 
NA 
Wd 

Im| 
四 
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c) 对 sigma 样本 点 进行 加 权 处 理 , 得 到 输 9 先 验 估 


计 。 其 中 系统 状态 的 均值 和 协 方差 估计 为 


2N _ 
DA (9) 
k=0 
i Cl =—(K)i ni =—(K)i Ai 
Pei > wh (ze Xp ) (z™ Np ) (10) 
k=0 


其 中 : ws) 和 w 分 别 为 第 上 个 sigma 点 的 权 值 的 均值 和 协 方差 。 
观测 量 的 均值 和 协 方差 估计 为 


2N > 
= OZ (11) 
k=0 
2N 加 和 AT 
Ps,=>w (20 -2)(2Y -2) (12) 
k=0 


系统 状态 与 观测 量 的 协 方差 估计 为 


RE (13) 
UPF 的 增益 为 
K,=P,,P, (14) 
d) 利用 浙 消 滤波 思想 采用 一 步 算 法 计算 渐 消 因子 
CQ =max {Lir(N,)ir(M,)) (15) 


其 中 : wr( ) 为 求 迹 符号 ;NN, 和 M, 在 非 线 性 滤波 中 计算 公式 如 


诸 
N,=VV, -HQH’ -R, (16) 
M, =HRP.FH (17) 

Oh (x of (x 

其 中 : V=% -h(x), H, = 0 » 让 一 

| ES 
e) 将 渐 消 滤波 与 UPF 相 结 合 ， 计 算出 更 新 后 状态 的 均值 
和 方差 : 

蕊 = 站 + 帮 (Z0 8) (18) 
P =Pa -KP (19) 


得 到 建议 分 布 函 数 N(x,P) 。 由 渐 消 因子 w 对 权 值 方差 进 
了 自 适应 调节 ， 可 以 充分 提取 观测 信息 中 的 有 效 信息 ， 抑 制 异 
常 干扰 对 状态 模型 和 观测 模型 的 不 利 影响 ， 使 建议 分 布 更 加 接 
近 真 实 后 验 分 布 。 
2.2 自 适 应 部 分 系统 (APS) 重 采 样 
自 适应 部 分 系统 重 采样 的 基本 思想 为 : 若 有 效 粒 子 数 小 于 
设 定 阔 值 , 则 根据 权 值 大 小 与 设 定 阔 值 相 比 来 对 粒子 进行 分 类 ， 
权 值 中 等 的 粒子 不 进行 重 采样 ， 权 值 过 大 和 过 小 的 粒子 进行 优 
化 组 合 ， 仅 对 组 合 后 的 部 分 粒子 进行 系统 重 采样 ， 从 而 降低 粒 
子 的 退化 。 具 体操 作 步 又 如 下 : 


ChinaXiv 合 作 期 刊 


王 倩 ， 等 : 基于 自 适应 渐 消 无 迹 粒 子 滤波 的 UnscentedFastSLAM 算法 


计算 粒子 集中 粒子 权 值 的 大 小 并 归 一 化 为 欢 ， 再 将 有 效 粒 
子 数 Ny 与 设 定 阔 值 w, 相 比 ， 若 Ny <N, 则 进行 重 采 样 ， 有 效 
粒子 数 由 式 (20) 计 算 。 


=1/ 站 (20) 


i=] 


根据 有 效 粒子 数 设 定 两 个 权 值 的 门限 值 w、w 
(0<w <w,)， 将 每 个 粒子 的 权 值 与 门限 值 一 一 比较 之 后 优化 组 
合 为 两 部 分 ， 一 部 分 为 <w 和 wi >w 的 权 值 较 小 或 者 较 大 的 
不 稳定 粒子 ， 另 一 部 分 为 w < 以 <w 的 权 值 适中 的 粒子 ， 仅 对 
权 值 不 稳定 的 那 部 分 粒子 进行 系统 重 采 样 后 ， 再 与 未 进行 重 采 
样 的 粒子 组 成 新 的 粒子 集 ， 通 过 这 种 分 配 组 合 能 够 增加 粒子 的 


多 样 性 。 

2.3 基于 AF-UPF 建议 分 布 和 APS 重 采样 的 UFastSLAM 方 
法 
提出 的 基于 AF-UPF 建议 分 布 和 APS 重 采 样 的 


UFastSLAM 算法 , 采用 自 适 应 渐 消 无 迹 粒子 滤波 器 完成 对 机 器 
人 位 姿 的 估计 ， 可 以 获得 更 接近 后 验 分 布 的 建议 分 布 函数 ， 采 
用 无 迹 卡 尔 曼 滤波 器 完成 对 路 标 位 置 的 估计 ， 采 用 自 适 应 部 分 
系统 重 采 样 的 方法 对 粒子 进行 重 采样 ,有 效 降低 粒子 退化 程度 。 
提出 算法 实现 过 程 如 下 : 

a) 初始 化 无 迹 粒子 滤波 器 。 根据 先 验 分 布 获得 初始 粒子 集 ， 
粒子 集中 粒子 数 为 Y ， 并 将 其 权 值 统一 分 配 为 I/N 。 

b) 重要 性 采样 。 根据 AF-UPF 计算 出 粒子 的 后 验 位 置 和 姿 
态 建议 分 布 ， 从 建议 分 布 中 采样 获得 新 的 粒子 集 ， 并 计算 粒子 
集中 粒子 的 归 一 化 权重 。 
c) 判断 新 的 环境 观测 值 是 否 为 地 图 中 已 存 特 征 路 标 。 如 果 
是 已 存 路 标 ， 则 采用 单一 兼容 最 近邻 方法 进行 数据 关联 ， 同 时 
更 新 机 器 人 位 姿 和 路 标 位 置 ， 如果 不 是 已 存 路 标 ， 则 将 新 的 路 
标 加 入 环境 地 图 中 。 
d) 重 采 样 。 在 有 效 粒子 数 小 于 设置 阔 值 时 ， 采 用 自 适 应 部 
分 系统 方法 对 粒子 进行 重 采 样 。 

3 ”仿真 结果 

为 了 验证 提出 算法 的 性 能 ， 利 用 Tim Bailey 公开 的 仿真 环 

境 和 MATLAB 2010a 仿真 软件 , 在 稀疏 路 标 环境 下 对 提出 算法 


和 UFastSLAM 算法 分 别 进行 仿真 实验 并 比较 。 仿 真 中 用 到 的 
运动 模型 和 观测 模型 如 式 (21) 和 (22) 所 示 。 


x XtV Art:cos(9,1+7) 
yi yt+V, Arsin(9,, +y) +6, (21) 
多 D + sin(7y) 


其 中 : (xy 多 ) 为 机 器 人 在 1 时 刻 的 位 姿 ; At 为 采样 时 间 间 隔 ; 
u=[V yx] 为 控制 输入 , 分 别 表 示 机 器 人 在 1 时 刻 的 速度 和 方向 
角 ; 5 为 运动 高 斯 噪 
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D 
a 


r (二 x+) 二 A 
2 i yi—y, 二 +6, (22) 。 2 | UFastSLAM 1 Je 
Hh 时 15 FP : i 
= 有 
其 中 ，(%,») 为 地 图 中 已 存 的 第 i 个 路 标的 坐标 ，(%,9) 为 传 感 Ro 
器 探测 到 该 路 标 相对 于 机 器 人 的 距离 和 方向 夹 角 ;6 为 服从 高 90 ono 8000 5000 


2000 3000 4000 5000 6000 7000 
时 间 / 步 


斯 分 布 的 观测 噪声 。 


下。 二 (a) 
仿真 实验 的 参数 配置 如 表 1 所 示 。 
表 1 仿真 参数 配置 下 对 i re 
y 1.5 HW 
参数 值 参数 值 2 c As 
ep 

机 器 人 轴 距 2m 最 大 观测 范围 30m os 人 和 
运动 速度 3m/s 运动 喝 声 0.4m/s,4° 0 1000 2000 和 3000 2 间 jE 6000 7000 8000 9000 
最 大 转向 角 。 30° 观测 噪声 0.2m,2° 


(b) 
图 3 ”两 种 算法 的 机 器 人 位 姿 估 计 x 向 、y 向 误差 


在 粒子 数 为 50 个 的 情况 下 ,对 UFastSLAM 和 本 文 提出 算 
法 分 别 进行 仿真 ,得 到 的 仿真 结果 如 图 1、2 所 示 。 其 中 蓝 色 线 | | | | 
条 为 实际 路 径 ， 红 色 线条 为 估计 路 径 ， 绿 色 星 号 为 实际 路 标 ， re iW 
红色 圆 点 为 估计 路 标 。 可 以 看 出 ， 本 文 提出 算法 得 到 的 估计 路 1 
径 和 实际 路 径 重 合 度 更 高 ， 对 路 标的 估计 也 更 准确 ， 都 具有 比 
UFastSLAM 算法 更 高 的 估计 精度 。 


机 


x 旬 万 同 汕 杯 仿 破 /m 


80 (a) 
60 入 每 - 村 
。 | | -一 提出 的 算法 大 
二 局 -二 -UFastSLAM | 2 
时 1.5 上 |- 
20 点 
加 || 
~ -20 6 
-40 
泛 (b) 
30 图 4 ”两 种 算法 的 路 标 位 置 估计 x 向、y 向 误差 
| 为 了 更 清楚 地 看 出 两 种 算法 的 估计 性 能 ， 利 用 机 器 人 位 姿 
图 1 UFastSLAM 算法 的 仿真 结果 
和 路 标 位 置 的 均 方 根 误差 (root mean square error, RMSE ) 来 衡 
量 ， 仿 真 结果 如 图 $ 所 示 。 其 中 蓝 色 柱状 代表 本 文 提 出 算法 ， 
80 
| 际 色 柱状 代表 UFastSLAM 算法 。 从 图 中 可 以 看 出 ， 本 文 提出 
30 1 篇 | 算法 在 机 器 人 位 姿 估 计 和 路 标 位 置 估计 两 方面 都 具有 更 高 的 估 
nl | 计 精 度 。 
E or \ 1 
国 \ 1.2- 
-20 - \ 起 | 
-40 记 
-60 区 ， -| E 0 9 加 | 
a0 | ， 六 
100 5 0 加 50 100 坚 
和 旦 0.6 
图 2 本文 提出 算法 的 仿真 结果 芭 
[em] 
3.1 50 个 粒子 时 的 算法 性 能 比较 ”0.3| ] 
粒子 数 为 50 个 时 ， 两 种 算法 的 机 器 人 位 姿 和 路 标 位 置 横 、 
纵 坐 标 估计 误差 曲线 如 图 3、4 所 示 。 可 以 看 出 , 虽然 本 文 提出 0 : 


位 姿 RMSE 路 标 RMSE 
图 5 两 种 算法 RMSE 


算法 有 小 部 分 误差 略 大 于 UFastSLAM 算法 ， 但 是 对 整个 地 医 
估计 和 机 器 人 路 径 估计 的 准确 度 明显 要 大 于 UFastSLAM 算法 。 
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3.2 ”粒子 数 不 同 时 的 性 能 比较 
在 粒子 数 分 别 为 10、30、50 个 时 ， 分 别 对 本 文 提出 算法 和 
UFastSLAM 算法 进行 20 次 仿真 实验 , 并 用 20 次 仿真 的 机 器 人 
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结束 语 


文中 提出 的 基于 自 适 应 渐 消 无 迹 粒 子 滤波 建议 分 布 ， 同 时 


位 姿 估 计 的 RMSE 均值 和 标准 差 作 为 评价 标准 , 实验 结果 如 图 
6 所 示 。 其 中 柱状 代表 均值 ， 线 棒 代表 标准 差 。 从 图 中 可 以 看 
出 ， 随 着 粒子 数 的 增多 ，UFastSLAM 算法 估计 精度 有 很 大 的 提 
升 ， 但 是 本 文 提 出 的 算法 RMSE 均值 变化 不 大 ， 也 就 是 说 提出 
算法 不 是 很 依赖 于 粒子 数 。 因 此 ， 本 文 提 出 算法 有 效 地 解决 了 
粒子 退化 的 问题 ， 可 以 用 较 少 的 粒子 达到 较 高 的 估计 精度 ， 在 
提高 计算 效率 的 同时 保证 了 估计 的 精度 。 


2 


国 国 改进 的 算法 
国 国 UFastSLAM 


a 


所 
a 


位 姿 估计 RMSE 的 均值 和 标准 差 /m 


10 50 


30 
粒子 数 / 个 
6 不 同 粒 子 数 时 两 种 算法 的 RMSE 


对 


3.3 ”算法 复杂 度 的 比较 

算法 复杂 度 的 对 比 可 以 通过 对 相同 粒子 数 时 两 种 算法 的 
RMSE 值 和 运行 时 间作 比较 ， 以 及 通过 比较 在 RMSE 值 差不多 
时 两 种 算法 所 需要 的 粒子 数 来 得 出 相应 的 结论 。 两 种 算法 的 对 
比分 别 如 表 2、3 所 示 。 表 2 显示 ,在 粒子 数 相同 时 ， 提 出 算法 
在 运行 时 间 上 和 UFastSLAM 差不多 ， 但 是 对 位 姿 估计 更 为 ;# 
确 ; 表 3 显示 ， 在 位 姿 估计 的 RMSE 均值 大 致 相同 时 ， 提 出 算 
法 需要 的 粒子 数 远 小 于 UFastSLAM。 因此 , 运用 提出 算法 能 够 
降低 计算 的 复杂 度 ， 提 高 算法 运行 效率 ， 很 好 地 解决 了 由 于 重 
采样 造成 的 粒子 退化 问题 。 


表 2 50 个 粒子 时 RMSE 和 运行 时 间 对 比 


I 


[E= 


RMSE/m 


算法 es 运行 时 间 /s 
均值 标准 闫 
提出 的 算法 0.7597 0.1024 172.256 
UFastSLAM 0.9831 0.1329 176.290 


表 3 RMSE 近似 相等 时 粒子 数 对 比 


RMSE/m 
算法 = 粒子 数 / 个 
均值 标准 差 
提出 的 算法 0.8155 0.1442 30 
UFastSLAM 0.8157 0.1301 70 


| 自 适 应 部 分 系统 重 采 样 的 UFastSLAM 方法 有 效 地 解决 了 


粒子 的 退化 问题 。 该 算法 在 采样 过 程 中 融入 渐 消 滤波 ， 通 过 浙 
消 因 子 产生 可 自 适应 调节 的 建议 分 布 函 数 ， 使 其 更 加 接近 后 验 


以 


分 布 函 数 ， 在 提高 粒子 采样 效率 的 同时 ， 根 据 粒 子 权重 大 小 将 
粒子 进行 分 配 、 优 化 组 合 后 再 对 部 分 粒子 进行 重 采 样 ， 保 证 了 


粒子 的 多 样 性 ， 避 免 了 粒子 的 贫 化 现象 。 通 过 仿真 实验 表明 ， 


与 UFastSLAM 相 比 ， 所 提 算 法 能 够 用 较 少 的 粒子 较 精 确 地 实 


现 忆 


器 人 的 自主 定位 和 构图 。 
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